#include "camera_device.h"
#include "lcd_display.h"
#define DBG
camera_device::camera_device(string video_device_,unsigned int image_width_,unsigned int image_height_,int buffer_count_,struct v4l2_format format)
{
	this->video_device = video_device_;
	this->image_width = image_width_;
	this->image_height = image_height_;
	image_size = image_width * image_height * 2;    //一幅图像YUV 格式的数据大小
	camera_device_fd = -1;
	buffer_count = buffer_count_;
	my_v4l2_format.type = format.type;
	my_v4l2_format.fmt.pix.pixelformat = format.fmt.pix.pixelformat;
	my_v4l2_format.fmt.pix.width = format.fmt.pix.width;
	my_v4l2_format.fmt.pix.height = format.fmt.pix.height;
	
	camera_image_buf = (char *)malloc(sizeof(char) * image_size);//yuv数据
	camera_rgb_buf   = (unsigned char *)malloc(sizeof(unsigned char) * image_height * image_width*4);  //rgb数据
	if(camera_image_buf == NULL)
	{
		cout << "malloc memory error!"<<endl;
		exit(EXIT_FAILURE);
	}
	memset(camera_image_buf,0,sizeof(char)*image_size);    //初始化数组
}
camera_device::~camera_device()
{
	free(camera_image_buf);
}
int camera_device::camera_open()
{
	camera_device_fd = open(video_device.c_str(), O_RDWR);//open("/dev/video0", O_RDWR);
	if (camera_device_fd < 0)
	{
		cout << "open video0 Faile"<<endl;
		return camera_device_fd;
	}
	return 0;
}
int camera_device::camera_close()
{
	close(camera_device_fd);
	return 0; 
}

int camera_device::camera_select(int index)
{
	int ret;
	int input = index;
	ret = ioctl(camera_device_fd, VIDIOC_S_INPUT, &input);
	return ret; 
}

int camera_device::camera_init()
{
	int ret;
	struct v4l2_format format;

	memset(&format, 0, sizeof(format));

	format.type                = my_v4l2_format.type;
	format.fmt.pix.pixelformat = my_v4l2_format.fmt.pix.pixelformat;
	format.fmt.pix.width       = my_v4l2_format.fmt.pix.width;
	format.fmt.pix.height      = my_v4l2_format.fmt.pix.height;

	ret = ioctl(camera_device_fd, VIDIOC_TRY_FMT, &format);
	
	if (ret != 0)
	{
		cout<< "can not VIDIOC_TRY_FMT!"<<endl;
		return ret;
	}

	format.type = my_v4l2_format.type;
	ret = ioctl(camera_device_fd, VIDIOC_S_FMT, &format);
	if (ret != 0)
	{
		cout<< "can not VIDIOC_S_FMT!"<<endl;
		return ret;
	}
	camera_mmap_init();
	return 0;
}
int camera_device::camera_mmap_init()
{
	int ret;
	struct v4l2_requestbuffers req;


	req.count = buffer_count;
	req.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
	//申请视频缓冲区，（这个缓冲区位于内核空间，需要mmap映射）
	//这一步操作可能会修改reqbuf.count的值，修改为实际成功申请缓冲区个数
	ret = ioctl(camera_device_fd,VIDIOC_REQBUFS,&req);
	if (ret != 0)
	{
		cout<< "can not VIDIOC_REQBUFS!"<<endl;
		return ret;
	}

	if (req.count < buffer_count)
	{
		cout<< "req.count < buffer_count"<<endl;
		return -1;
	}

    user_buf = (BUFTYPE *)calloc(req.count,sizeof(* user_buf));
	if (user_buf == NULL)
	{
		fprintf(stderr,"Out of memory\n");
		exit(EXIT_FAILURE);
	}
	//将内核缓冲区映射到用户进程空间
	for(int i = 0;i < buffer_count;i++)
	{
		struct v4l2_buffer buf;
		bzero(&buf,sizeof(buf));
		
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index  = i;
		
		//查询申请到内核缓冲区的信息
		ret = ioctl(camera_device_fd,VIDIOC_QUERYBUF,&buf);
		if(ret != 0)
		{
			perror("Fail to ioctl : VIDIOC_QUERYBUF");
            exit(EXIT_FAILURE);
		}
		user_buf[i].length = buf.length;
		user_buf[i].start = mmap(NULL,
								buf.length,
								PROT_READ | PROT_WRITE,
								MAP_SHARED,
								camera_device_fd,
								buf.m.offset
								);
		if(MAP_FAILED == user_buf[i].start)
		{
			perror("failed to mmap!");
			exit(EXIT_FAILURE);
		}
	}
	return 0;
}
void camera_device::camera_capture_start()
{
	unsigned int i;
	int ret=-1;
	enum v4l2_buf_type type;

	for(i = 0; i < buffer_count; i++)
	{
		struct v4l2_buffer   buf;
		bzero(&buf,sizeof(buf));
		buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index  = i;

		ret = ioctl(camera_device_fd,VIDIOC_QBUF,&buf);
		if (ret != 0)
		{
			perror("Fail to ioctl 'VIDIOC_QBUF'");
			exit(EXIT_FAILURE);
		}
	}

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	ret = ioctl(camera_device_fd,VIDIOC_STREAMON,&type);
	if (ret != 0)
	{
        printf("i = %d.\n",i);
        perror("Fail to ioctl 'VIDIOC_STREAMON'");
        exit(EXIT_FAILURE);
	}
}
void camera_device::camera_capture_stop()
{
    enum v4l2_buf_type type;
 
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(-1 == ioctl(camera_device_fd,VIDIOC_STREAMOFF,&type))
    {
        perror("Fail to ioctl 'VIDIOC_STREAMOFF'");
        exit(EXIT_FAILURE);
    }
}

int camera_device::camera_read_frame()
{
    int ret;
    struct v4l2_buffer buffer;
    

    memset(&buffer, 0, sizeof(buffer));
    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buffer.memory = V4L2_MEMORY_MMAP;
    buffer.index = buffer_count;
    ret = ioctl(camera_device_fd, VIDIOC_DQBUF, &buffer);
    if (ret != 0)
    {
        //DBG("ioctl(VIDIOC_DQBUF) failed %d(%s)\n", errno, strerror(errno));
        return ret;
    }
	ret = buffer.index;
    if (ret < 0 || ret >= buffer_count)
    {
       cout<<"invalid buffer index: %d"<<endl;
        return ret;
   }

	memcpy(camera_image_buf, user_buf[buffer.index].start,image_size);  //复制数据到缓存
    
	ret = ioctl(camera_device_fd, VIDIOC_QBUF, &buffer);
    if (ret != 0)
    {
		cout<<"ioctl(VIDIOC_QBUF) failed %d(%s)\n"<<endl;
		return ret;
    }
    return 0;
}
void camera_device::get_camera_image_data(char *data)
{
	memcpy(data, camera_image_buf,image_height*image_width*2);
}
void camera_device::get_camera_image_data(unsigned char *data)
{
	memcpy(data, camera_image_buf,image_height*image_width*2);
}
int camera_device::clip(int value, int min, int max)
{
	return (value > max ? max : value < min ? min : value);
}
void camera_device::yum2rgb(void *data)
{
	unsigned char* in=(unsigned char*)data;
	int width=image_width;
	int height=image_height;
	int x = 0,y = 0,j = 0;

	int y0,u,y1,v,r,g,b;
	long location=0;
	for ( y = 0; y < height; ++y)//480
	{
		for (j = 0; j < width * 2; j += 4)//640
		{
			y0= in[x];
			u= in[x + 1] - 128;
			y1	= in[x + 2];
			v	= in[x + 3] - 128;

			r = (298 * y0 + 409 * v + 128) >> 8;
			g = (298 * y0 - 100 * u - 208 * v + 128) >> 8;
			b = (298 * y0 + 516 * u + 128) >> 8;

			camera_rgb_buf[ location + 0] = clip(b, 0, 255);
			camera_rgb_buf[ location + 1] = clip(g, 0, 255);
			camera_rgb_buf[ location + 2] = clip(r, 0, 255);

			r = (298 * y1 + 409 * v + 128) >> 8;
			g = (298 * y1 - 100 * u - 208 * v + 128) >> 8;
			b = (298 * y1 + 516 * u + 128) >> 8;

			camera_rgb_buf[ location + 3] = clip(b, 0, 255);
			camera_rgb_buf[ location + 4] = clip(g, 0, 255);
			camera_rgb_buf[ location + 5] = clip(r, 0, 255);

			location = location + 6;
			x = x + 4;
		}
	}
}
void camera_device::get_camera_rgb_data(unsigned char *data)
{
	camera_read_frame();  //读取数据
	yum2rgb(camera_image_buf);//数据转换
	memcpy(data, camera_rgb_buf,image_height*image_width*4);  //复制数据到缓存
}
void camera_device::get_camera_yuv_data(unsigned char *data)
{
	camera_read_frame();  //读取数据
	memcpy(data, camera_image_buf,image_height*image_width*2);
}
void camera_device::get_camera_yuv_data(char *data)
{
	camera_read_frame();  //读取数据
	memcpy(data, camera_image_buf,image_height*image_width*2);
}
#if 0
int main(int argc, char const *argv[])
{
	struct v4l2_format format;
	char image_data[640*480*2]={0};
	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    format.fmt.pix.width = 640;
    format.fmt.pix.height = 480;
	camera_device t("/dev/video0",640,480,4,format);
	lcd_display   t1("/dev/fb0");
	t1.lcd_init();
	t.camera_open();
	cout<< 1 << endl;
	
	t.camera_select(0);
	cout << 2 <<endl;

	t.camera_init();
	cout << 3 << endl;
	t.camera_capture_start();
	
	printf("init done!\n");
	
	//t.camera_capture_stop();
	while(1)
	{
		t.get_camera_yuv_data(image_data);
		t1.lcd_show(640,480,image_data);
	}
	printf("%s\n", "dfdfddfdf");
	return 0;
}
#endif
